#pragma ident "$Id: ARRound.hpp ?? 2012-01-19 14:42:13Z shjzhang $"

/**
 * @file ARRound.hpp
 *
 */

#ifndef GPSTK_ARRound_HPP
#define GPSTK_ARRound_HPP

//============================================================================
//  Revision
//
//  - Create this program, 2012.01.19
//  - Modify some variable names, which is more meaningful. 2012.06.19
//  - Change the 'ARRound' from the inherited class of ARBase to independent
//    one, 2014.03.13, shjzhang.
//  - redesign the interface for ambigutiy resolution
//    2019/08/03, Shoujian, Zhang.
//
//============================================================================

#include "Matrix.hpp"
#include "Exception.hpp"
#include "SpecialFunctions.hpp"

using namespace std;


namespace gpstk
{

    /// New exception for Ambiguity Resolution
    NEW_EXCEPTION_CLASS(ARRoundException, Exception);

    struct ARData
    {
        ARData()
            : ambFixed(0),fixed(false),precision(0.0)
        {}

        double ambFixed;
        bool fixed;
        double precision;
    };


      /** This class resolve integer ambiguity using the rounding criterion
       *  based on a decision function adobted by (Dong D., Bock Y., 1988).
       */
    class ARRound
    {
    public:

         /// Default constructor
        ARRound()
            : cutDec(1000.0), cutDev(0.2), cutSig(0.1)
        {};


        ARRound(double cutDeviation,
                double cutSigma,
                double cutDecision=1000)
            : cutDec(cutDecision), cutDev(cutDeviation), cutSig(cutSigma)
        {};


         /** Resolve the ambiguities to integers according to the round
          *  method from Dong Danan's paper.
          *
          * @param ambFloat         Float ambiguities
          * @param ambCov           Ambiguities' covariance
          */
        virtual ARData resolve(const double ambFloat,
                               const double ambCov)
            noexcept(false)
        {


            double ambFixed, sigma, decision;

            // get the neareast integer of the float ambiguity
            ambFixed = std::floor( ambFloat + 0.5 );

            // Get sigma
            sigma = std::sqrt(ambCov);

            // Compute the decision for ambiguity
            decision = getDecision(ambFloat, sigma);


            // Fix the ambiguities with reasonable condition
            bool fixed(false);
            if( decision > cutDec )
            {
                // update the ambiguity flags
                fixed = true;
            }

            ARData arData;
            arData.ambFixed = ambFixed;
            arData.fixed = fixed;
            arData.precision = decision;

            return arData;

        };  // End of method 'ARRound::resolve()'


         /** Compute and return the decision of the ambiguity according to the
          *  decision fucntion.
          *
          * @param ambiguity        Float ambiguity
          * @param sigma            Variance of the ambiguity
          *
          * @return the decision values
          */
        double getDecision(double value, double sigma )
        {
            double INT, dev;

            // Integer of the float ambiguity
            INT = std::floor( value+0.5 );

            // Deviation of the float ambigity and the integer ones.
            dev = std::abs  ( value-INT );

            // Taper function variable
            double t1, t2, taper;


            // Compute the taper function
            if( dev > cutDev )
            {
                t1 = 0.0;
            }
            else
            {
                t1 = (1-dev/cutDev);
            }

            // t2
            if( sigma > cutSig )
            {
                t2 = 0.0;
            }
            else
            {
                t2 = ( 1 - 3*sigma );
            }


            // computing taper function
            taper = std::pow(t1,2)*t2;

            double Q0(0.0);
            double b1(0.0), b2(0.0);
            double e1(0.0), e2(0.0);
            double de;

            // Compute the Q(0) according to equation A-12 in Dong and Bock
            for( int i=1; i<50; i++ )
            {
                // b1
                b1 = (i-dev)/(std::sqrt(2)*sigma);

                // b2
                b2 = (i+dev)/(std::sqrt(2)*sigma);

                // e1
                if( b1<0.0 || b1>15.0 ) { e1 = 0.0; }
                else                    { e1 = erfc(b1); }

                // e2
                if( b2<0.0 || b2>15.0 ) { e2 = 0.0; }
                else                    { e2 = erfc(b2); }

                // de
                de = e1 - e2;

                // rounding criterion values
                Q0 = Q0 + de;

                // break if the de less than a small value
                if(de < 1.0E-8) { break; }
            }

            double decision;

            if(Q0 < 1.0E-8) { Q0 = 1.0E-8; }

            // threshold decision
            decision = taper/Q0;

            // Reset the decision if it is less than 'cutDec'
            if(decision <= cutDec )
            {
                decision = 1.0E-8;
            }

            return decision;

        };  // End of method 'ARRound::getDecision()'


         /** Set the ambiguity cutting decision.
          *
          * @param cutDeviation     Cutting decision
          */
        void setCutDev(double cutDeviation)
        {
            cutDev = cutDeviation;
        };


         /* Set the cutting sigma.
          *
          *@param cutSigma          Cutting sigma for the ambiguity fixing
          */
        void setCutSig(double cutSigma)
        {
            cutSig = cutSigma;
        };

        /// Destructor.
        virtual ~ARRound() {};


    private:

        // Threshold decision for fixing ambiguity
        double cutDec;

        // Threshold deviation for taper function
        double cutDev;

        // Threshold sigma for taper function
        double cutSig;


    };   // End of class 'ARRound'

}   // End of namespace gpstk


#endif  //GPSTK_AR_ROUND_HPP
